/*	IMU.c & .h
 *	
 *	HUV Robotics IMU interface module.
 *	The raw gyro & accelerometer data from the HUV Robotics IMU is combined to
 *	calculate Pitch & Roll.  Outputs are calibrated in radians.
 *	The module does not provide any yaw feedback (even though the IMU has a a yaw
 *	gyro) since there is no compass to cancel out yaw gyro drift.
 *	The module expects IMUInitialise() to be called at the start of the program then
 *	IMUHeartBeat() to be called 128 times per second.  IMU output can then be read
 *	via IMURead().
 *	IMURadiansToDegrees() can be used if output in degrees is required.
 *
 *	Author:  Phil Hall
 *	Date:    May 2008
 *
*/

/*
	Complementary filter added 06/07/09
	Alexandre Vicente
*/

#include <inttypes.h>
#include <avr/io.h>
#include <avr/interrupt.h>
#include <avr/signal.h>
#include <string.h>
#include <stdio.h>
#include <math.h>

#include "AX-12.h"
#include "IMU.h"


static unsigned char 	_IMU_ID = DEFAULT_IMU_ID;
static IMU_RAW			Raw;
static IMU_DATA			_IMUData;
static int				IMUStatus = 0;
static int				_IMUTestMode = 0;
static IMU_RAW			IMUZero;

static unsigned short	PitchMin = 270,		//	Pitch accelerometer read when robot laying face down
						PitchMax = 408,		//	Pitch accelerometer read when robot laying face up
						RollMin = 265,		//	Roll accelerometer read when robot laying on right arm
						RollMax = 400;		//	Roll accelerometer read when robot laying on left arm

static double			PitchDelta90  = 0,				//	Calculated from Min & Max values
						PitchAccum180 = 3.14/-1493,		//	The denominator is the average pitch gyro accumulator count for 10 clockwise & 10 anticlockwise 180 degree (pi radian) turns
						RollDelta90   = 0,				//	Calculated from Min & Max values
						RollAccum180  = 3.14/1483;		//	The denominator is the average roll gyro accumulator count for 10 clockwise & 10 anticlockwise 180 degree (pi radian) turns

//---------------Complementary_Filter_variables-----------------
static double 	Pitch_Old,Roll_Old;
const double 	SamplingFrequency 	= 128.0;//Hz
const double 	CutOffFrequency 	= 0.1;//Hz
const short	ADCresolution		= 1024;//ADC units
const short	ATMega_VREF			= 3300;//mV
const double	Gyro_X_Sensitivity	= 1.3;//mV/degree/second
const double	Gyro_Y_Sensitivity	= 1.3;//mV/degree/second
double	IntegralConstantX;  //= 0.00044978;//0.025771;
double	IntegralConstantY; 	//= 0.00044978;//0.025771;
double Alpha,InvAlpha,Period,RC;
double Pitch_Raw,
		Pitch_Gyro,
		Pitch,
		Roll_Raw,
		Roll_Gyro,
		Roll;

//--------------------------------------------------------------


int IMUInitialise(unsigned char IMU_ID)
	//	Set IMU_ID to zero to use DEFAULT_IMU_ID
{
	_IMU_ID = (IMU_ID != 0)? IMU_ID: DEFAULT_IMU_ID;

	memset(&IMUZero, 0, sizeof(IMUZero));
	
	IMUZero.AccelFore = (PitchMin + PitchMax) / 2;
	PitchDelta90 = (double)( (double)(PitchMax - PitchMin) / 2);

	IMUZero.AccelSide = (RollMin + RollMax) / 2;
	RollDelta90 = (double)( (double)(RollMax - RollMin) / 2);
	
	
	// new alex	
	Pitch_Gyro = 0;//Complementary Filter component
	Roll_Gyro = 0;//Complementary Filter component
	IntegralConstantX = ((double)ATMega_VREF/(SamplingFrequency*Gyro_X_Sensitivity*(double)ADCresolution))*(3.14159265/180);
	IntegralConstantY = ((double)ATMega_VREF/(SamplingFrequency*Gyro_Y_Sensitivity*(double)ADCresolution))*(3.14159265/180);
	
	//Low Pass calculation
	Period = 1.0f/SamplingFrequency;
	RC = 1.0f/(6.283185*CutOffFrequency);
	Alpha = Period/(Period + RC);
	InvAlpha = 1-Alpha;
	
	Pitch_Old = 0;
	Roll_Old = 0;

	return 0;
}


void IMUGetRaw(unsigned char IMUID, IMU_RAW *Raw)
{
	// unchanged by alex
	unsigned char cData [40];
	
	if (IMUID == 0)
		IMUID = DEFAULT_IMU_ID;

	IMUStatus = AX12ReadMemory(IMUID, 0x1A, 12, cData);
//	CommsPrintf("Status %d", IMUStatus);
	
	if (IMUStatus > 0)
	{
		Raw->AccelSide =  (cData[1]  << 8) + cData[0];
		Raw->AccelFore =  (cData[3]  << 8) + cData[2];
		Raw->AccelVert =  (cData[5]  << 8) + cData[4];
		Raw->GyroPitch =  (cData[7]  << 8) + cData[6];
		Raw->GyroRoll =   (cData[9]  << 8) + cData[8];
		Raw->GyroYaw =    (cData[11] << 8) + cData[10];
		//Raw->Dig1  =     cData[12];
		//Raw->Dig2  =     cData[13];
		//Raw->Dig3  =    cData[14];
		//Raw->Dig4  =    cData[15];
	}
}

void IMUGetFiltered(unsigned char IMUID, AXIS *Axis,double Offset_Pitch,double Offset_Roll) //The Complementary Filter
{
// new by alex
// uses new structure Axis.
			
	unsigned char cData [40];
	short X_ADC,Y_ADC,Z_ADC,Pitch_ADC,Roll_ADC;
	
	if (IMUID == 0)
		IMUID = DEFAULT_IMU_ID;

	IMUStatus = AX12ReadMemory(IMUID, 0x1A, 12, cData);
	
	// gets raw data	
	if (IMUStatus > 0)
	{
		X_ADC 		=  ((cData[1]  << 8) + cData[0])-X_zero;
		Y_ADC 		=  ((cData[3]  << 8) + cData[2])-Y_zero;
		Z_ADC 		=  ((cData[5]  << 8) + cData[4])-Z_zero;
		Pitch_ADC 	=  ((cData[7]  << 8) + cData[6])-Pitch_zero;
		Roll_ADC 	=  ((cData[9]  << 8) + cData[8])-Roll_zero;
	}	

	//PITCH calibration
	Pitch_Raw = atan2((double)(Y_ADC),(double)(Z_ADC));
	Pitch_Gyro = IntegralConstantX*(double)Pitch_ADC;
	Pitch = Alpha*(Pitch_Raw + Pitch_Gyro) + InvAlpha*Pitch_Old - Pitch_Gyro;
	Pitch_Old = Pitch;

	
	//ROLL calibration
	Roll_Raw = atan2((double)(X_ADC),(double)(Z_ADC));
	Roll_Gyro = IntegralConstantY*(double)Roll_ADC;
	Roll = Alpha*(Roll_Raw - Roll_Gyro) + InvAlpha*Roll_Old + Roll_Gyro;
	Roll_Old = Roll;


	//Leaning direction
	//
	
	//Convert output to degrees - to send integer numbers to the terminal
	//not needed if outputs in rad only is required.
	if(1)
	{
		Pitch_Raw *= 572.957;
		Pitch *= 572.957; 
		Roll_Raw *= 572.957;
		Roll *= 572.957;
	}
		

	//CommsPrintf("%d,%d,%d,%d,%d\r\n",X,Y,Z,Pitch,Roll);
	
	//Write output values to target address
	Axis->Pitch = Pitch;//Offset
	Axis->Pitch_Raw = Pitch_Raw;
	Axis->Roll = Roll;//Offset
	Axis->Roll_Raw = Roll_Raw;
	//Axis->Vector_Angle = (atan2(Pitch,Roll))*572.957;
	//Axis->Vector_Mag = sqrt(Pitch*Pitch+Roll*Roll);

}

int iabs(int i)	//	Returns the absolute value of i
{
	return (i < 0)? i * -1: i;
}


static int IMUCanZeroPitch(IMU_RAW *Raw)
{
	static unsigned	LastAccel = 0,
						LastGyro = 0,
						SimilarCount = 0;

	if (iabs(LastAccel - Raw->AccelFore) < 3)
	{
		if (iabs(LastGyro - Raw->GyroPitch) < 3)
		{
			if (SimilarCount < 1000)
				SimilarCount ++;
		}
		else
			SimilarCount = 0;
	}
	else
		SimilarCount = 0;

	LastAccel = Raw->AccelFore;
	LastGyro  = Raw->GyroPitch;
	
	return (SimilarCount > 5)? 1: 0;
}

static int IMUCanZeroRoll(IMU_RAW *Raw)
{
	static unsigned	LastAccel = 0,
						LastGyro = 0,
						SimilarCount = 0;

	if (iabs(LastAccel - Raw->AccelSide) < 3)
	{
		if (iabs(LastGyro - Raw->GyroRoll) < 3)
		{
			if (SimilarCount < 1000)
				SimilarCount ++;
		}
		else
			SimilarCount = 0;
	}
	else
		SimilarCount = 0;

	LastAccel = Raw->AccelSide;
	LastGyro  = Raw->GyroRoll;
	
	return (SimilarCount > 5)? 1: 0;
}


double IMUArcSin(double V)
{
	if (V < -1)	V = -1;
	if (V > 1)		V = 1;
	
	return(asin(V));
}

static void IMUReadPosSpeed (unsigned short ServoID, unsigned short *Pos, unsigned short *Speed)
{
	unsigned char cData [40];
	
	IMUStatus = AX12ReadMemory(ServoID, AX12_PRESENT_POSITION_L, 4, cData);
	
	if (IMUStatus > 0)
	{
		*Pos =   (cData[1] << 8) + cData[0];
		*Speed = (cData[3] << 8) + cData[2];
	}
}

static void IMUTestHeartBeat(void)
{
	static int tick=-128;
	IMU_DATA	IMU;
	unsigned short Pos = 0, Speed = 0;
	double Val1, Val2;
	static int NewPos = 4547;
	unsigned short ServoID = 6;
	
	tick ++;
	
	if (_IMUTestMode == 1)
	{
		if (tick == -1)
			CommsPrintf ("Servo Pos, IMU Pos, Servo Speed\r\n");
			
		if ( (tick > 0) && ( (tick % 4) == 0) )
		{
			IMURead(&IMU);
			
			IMUReadPosSpeed (ServoID, &Pos, &Speed);
			
			Val1 = (double)Pos;
			Val1 = Val1 / 1024 * 300;
			Val1 -= 150;
			Val1 *= -1;
			Val1 += (Val1 < 0)? -0.5: 0.5;
			
			Val2 = (double)Speed;
			Val2 = Val2 / 1024 * 300;	//	Make the scale similar to the angular measures
			
			CommsPrintf("%d,%d,%d\r\n", (int)Val1, IMURadiansToDegrees(IMU.Pitch), (int)Val2);
		}
		
		if (tick == 64)
		{
			tick = 0;
			NewPos *= 1255;	//	This is an attempt at a random number generator
			Pos = (unsigned short)(NewPos & 0x3FF);
			if (Pos < 212) Pos = 212;	//	Limit movements to 212 (i.e. 512 - 300) to 812 (512 + 300)
			if (Pos > 812) Pos = 812;	//	This will keep the pitch & roll accelerometers up the right way
			AX12SetPosition(ServoID, Pos, (unsigned short)(NewPos & 0x1FF));
		}
	}
}


void IMUHeartBeat(void)
{
	static int Tick = 1;
	double Val;
	
	Tick --;
	
	if (Tick == 0)
	{
		Tick = IMU_HB_TICKS;
		IMUGetRaw(_IMU_ID, &Raw);

		//	Pitch section 
		
		if (IMUZero.GyroPitch != 0)	//	This is true when the gyro has been initialised
		{
			Val  = (double)(Raw.GyroPitch) - (double)(IMUZero.GyroPitch);
			Val *= PitchAccum180;
			_IMUData.Pitch += Val;
		}

		if (IMUCanZeroPitch(&Raw))			//	This is true when the accelerometer and gyro have been stable for several reads
			IMUZero.GyroPitch = Raw.GyroPitch;

		if ( (iabs(Raw.GyroPitch - IMUZero.GyroPitch) < 2) ||	//	This is true when the gyro isn't seeing any movement
			 (IMUZero.GyroPitch == 0) )						//	and this is true when the gyro hasn't been initialised yet
		{
			Val = (double)((double)(Raw.AccelFore) - (double)(IMUZero.AccelFore));	//	It really seems to need all this casting! :o(  
			Val /= PitchDelta90;
			_IMUData.Pitch = IMUArcSin(Val);
		}			

		//	Roll section 
		
		if (IMUZero.GyroRoll != 0)	//	This is true when the gyro has been initialised
		{
			Val  = (double)(Raw.GyroRoll) - (double)(IMUZero.GyroRoll);
			Val *= RollAccum180;
			_IMUData.Roll += Val;
		}

		if (IMUCanZeroRoll(&Raw))			//	This is true when the accelerometer and gyro have been stable for several reads
			IMUZero.GyroRoll = Raw.GyroRoll;

		if ( (iabs(Raw.GyroRoll - IMUZero.GyroRoll) < 2) ||	//	This is true when the gyro isn't seeing any movement
			 (IMUZero.GyroRoll == 0) )						//	and this is true when the gyro hasn't been initialised yet
		{
			Val =  (double)((double)(Raw.AccelSide) - (double)(IMUZero.AccelSide));	//	It really seems to need all this casting! :o(  
			Val /= RollDelta90;
			_IMUData.Roll = IMUArcSin(Val);
		}
	}
	
	if (_IMUTestMode)
		IMUTestHeartBeat();
}

int IMURead(IMU_DATA *IMUData)
{
	memcpy (IMUData, &_IMUData, sizeof(IMU_DATA));
	return IMUStatus;
}

int IMURadiansToDegrees(double Radians)
{
	return (int)((Radians * 57.3)+0.5);
}

void IMUTestMode (int Mode)
{
	_IMUTestMode = Mode;
}

// new alex
//Digital Low Pass filter for Pitch - Complementary Filter Component
double LowPass_Pitch(double In)
{
	static double Out_Old = 0;
	double Out;

	Out = Alpha*In + (1-Alpha)*Out_Old;

	Out_Old = Out;
	return Out;

}

// new alex
//Digital Low Pass filter for Roll - Complementary Filter Component
double LowPass_Roll(double In)
{
	static double Out_Old = 0;
	double Out;

	Out = Alpha*In + (1-Alpha)*Out_Old;

	Out_Old = Out;
	return Out;

}
